/**
 * @file core.c
 * @author your name (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2022-07-15
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "k2os.h"



/*
 ***************************************************************************************
 *                             global variables 
 *************************************************************************************** 
 */
u8_t                        k2os_isr_nesting;                                       /* os isr nesting value */
u8_t                        k2os_running;                                           /* os running flag */ 
volatile u16_t              k2os_curr_running_priority;                             /* current running task's priority */
volatile u16_t              k2os_next_run_priority;                                 /* next run priority */  
u8_t                        k2os_task_ready_group;
u8_t                        k2os_task_ready_tbl[OS_CFG_TASK_READY_TBL_SIZE];     
volatile os_tcb_t           *k2os_curr_running_task_ptcb;                           /* */
volatile os_tcb_t           *k2os_next_run_task_ptcb;                               /**/
volatile u32_t              k2os_ticks;
      

/**
 * @brief clear a block memory
 * 
 * @param src   the start of ram to clear
 * @param size  the number of bytes to clear
 */
static void os_mem_clear(u8_t *src, u16_t size)
{
    while (size > 0){
        *src++ = (u8_t)0;
        size--;
    }
}

u8_t os_get_running_status(void)
{
    return k2os_running;
}


/*
 **********************************************************************************************
 *               tcbs list function, add, delete, motify 
 ********************************************************************************************** 
 */
static os_tcb_t *k2os_list_head_tcbs = NULL;
static os_tcb_t *k2os_list_tcbs_tail = NULL;


static void tcb_slist_add_tail(os_tcb_t *node)
{
    os_tcb_t *ptcb = NULL;

    if(k2os_list_head_tcbs == NULL){
        k2os_list_head_tcbs = node;
        k2os_list_tcbs_tail = node;
        return;
    }

    ptcb = k2os_list_tcbs_tail;
    ptcb->list_next_tcb = node;
    k2os_list_tcbs_tail = node;
}

#define tcb_slist_for_each_entry(node)              \
    for(node = k2os_list_head_tcbs; node; node = node->list_next_tcb)


#define tcb_slist_for_each_entry_safe(tmp, node)    \
    for(node = k2os_list_head_tcbs,                 \
        tmp = k2os_list_head_tcbs ? k2os_list_head_tcbs->list_next_tcb : NULL; \
        node;                                       \
        node = tmp, \
        tmp = tmp ? tmp->list_next_tcb : tmp)


/*
static void tcb_slist_del(os_tcb_t *node)
{
    os_tcb_t *ptmp = NULL, *ptcb = NULL;

    if(k2os_list_head_tcbs == node){
        k2os_list_head_tcbs = node->list_next_tcb;
        // update the tcb tail 
        if(k2os_list_head_tcbs == NULL){
            k2os_list_tcbs_tail = NULL;
        }

        return;
    }

    tcb_slist_for_each_entry_safe(ptmp, ptcb){
        if(ptmp == node){
            ptcb->list_next_tcb = node->list_next_tcb;
             // update the tcb tail 
            if(ptcb->list_next_tcb == NULL){
                k2os_list_tcbs_tail = ptcb;
            }

            return;
        }
    }
}
*/


/*
 **********************************************************************************************
 *                             global task tcb data heap function  
 ********************************************************************************************** 
 */
static os_tcb_heap_t    k2os_tcb_heaps[OS_CFG_TASKS_MAX + OS_SYSTEM_TASKS_NUM];

void os_tcb_heaps_init(void)
{
    os_mem_clear((u8_t *)&k2os_tcb_heaps[0], 
                 (OS_CFG_TASKS_MAX + OS_SYSTEM_TASKS_NUM) * sizeof(os_tcb_heap_t));

    k2os_list_head_tcbs = NULL;
    k2os_list_tcbs_tail = NULL;
}

os_tcb_t *os_tcb_malloc(void)
{
    u16_t i;
    os_tcb_t *ptcb = NULL;
    for(i = 0; i < (OS_CFG_TASKS_MAX + OS_SYSTEM_TASKS_NUM); i++){
        if(k2os_tcb_heaps[i].flag_used == OS_FLAG_UNUSED){
            k2os_tcb_heaps[i].flag_used = OS_FLAG_USED;
            ptcb = &k2os_tcb_heaps[i].tcb;
            
            ptcb->list_next_tcb = NULL;
            
            return ptcb;
        }
    }

    return (os_tcb_t *)0;
}


void os_tcb_free(os_tcb_t *tcb)
{
    u16_t i;
    for(i = 0; i < (OS_CFG_TASKS_MAX + OS_SYSTEM_TASKS_NUM); i++){
        if(tcb == &k2os_tcb_heaps[i].tcb){
            k2os_tcb_heaps[i].flag_used = OS_FLAG_UNUSED;
            os_mem_clear((u8_t *)tcb, sizeof(os_tcb_t));
        }
    }
}

/*
 **********************************************************************************************
 *                             global task priority table func
 ********************************************************************************************** 
 */
static os_task_priority_info_t          k2os_task_priority_info_tbl[OS_CFG_LOWEST_PRIORITY + 1];

void os_task_priority_info_tbl_init(void)
{
    u16_t i;
    for(i = 0; i < (OS_CFG_LOWEST_PRIORITY + 1); i++){
        os_mem_clear((u8_t *)&k2os_task_priority_info_tbl[0], 
                     (OS_CFG_LOWEST_PRIORITY + 1) * sizeof(os_task_priority_info_t));
    }
}

s16_t os_task_priority_info_tbl_insert(u8_t priority, os_tcb_t *ptcb)
{
    if((priority > OS_CFG_LOWEST_PRIORITY) || 
       (priority == 0)){
        return OS_ERR_PRIORITY_INVALID;
    }

    if(k2os_task_priority_info_tbl[priority].ptcb != NULL){
        return OS_ERR_PRIORITY_EXIST;
    }

    k2os_task_priority_info_tbl[priority].ptcb = ptcb;

    return OS_ERR_NONE;
}

s16_t os_task_priority_info_tbl_delete(u8_t priority)
{
    if((priority > OS_CFG_LOWEST_PRIORITY) || 
       (priority == 0)){
        return OS_ERR_PRIORITY_INVALID;
    }

    k2os_task_priority_info_tbl[priority].ptcb = (os_tcb_t *)0;

    return OS_ERR_NONE;
}


/*
 **********************************************************************************************
 *                             find the highest ready task for schedule 
 ********************************************************************************************** 
 */
const u8_t k2os_lowest_bit_of_byte_map[256] = {
    0u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x00 to 0x0F */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x10 to 0x1F */
    5u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x20 to 0x2F */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x30 to 0x3F */
    6u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x40 to 0x4F */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x50 to 0x5F */
    5u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x60 to 0x6F */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x70 to 0x7F */
    7u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x80 to 0x8F */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0x90 to 0x9F */
    5u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0xA0 to 0xAF */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0xB0 to 0xBF */
    6u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0xC0 to 0xCF */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0xD0 to 0xDF */
    5u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, /* 0xE0 to 0xEF */
    4u, 0u, 1u, 0u, 2u, 0u, 1u, 0u, 3u, 0u, 1u, 0u, 2u, 0u, 1u, 0u  /* 0xF0 to 0xFF */
};

static u8_t os_get_highest_ready_task_priority(void)
{
    u8_t x, y;

    y = k2os_lowest_bit_of_byte_map[k2os_task_ready_group]; 
    x = k2os_lowest_bit_of_byte_map[k2os_task_ready_tbl[y]];

    k2os_next_run_priority = (u8_t)((y << 3) + x);

    return k2os_next_run_priority;
}

static void os_update_next_run_task_ptcb(void)
{
    k2os_next_run_task_ptcb = k2os_task_priority_info_tbl[k2os_next_run_priority].ptcb;
}




static s16_t os_task_tcb_init(os_tcb_t *ptcb, u8_t priority, os_stk_t *pstack)
{
    u16_t rc;
    if((ptcb == NULL) || (pstack == NULL)){
        return OS_ERR_TCB_INIT_PARAM_ILLEGAL;
    }

    if((priority == 0) || (priority > OS_CFG_LOWEST_PRIORITY)){
        return OS_ERR_TCB_INIT_PARAM_ILLEGAL;
    }

    ptcb->pTopOfStack = pstack;
    ptcb->taskPriority = priority;

    ptcb->taskReadyGroup_bit = (priority >> 3);   // priority / 8
    ptcb->taskReadyTbl_bit = (priority & 0x07);     

    ptcb->taskReadyGroup_mask = (1 << ptcb->taskReadyGroup_bit);
    ptcb->taskReadyTbl_mask = (1 << ptcb->taskReadyTbl_bit);

    ptcb->taskStatus = OS_TASK_STATUS_READY;
    ptcb->taskDlyTicks = 0;

    k2os_task_ready_group |= ptcb->taskReadyGroup_mask;
    k2os_task_ready_tbl[ptcb->taskReadyGroup_bit] |= ptcb->taskReadyTbl_mask;

    tcb_slist_add_tail(ptcb);

    rc = os_task_priority_info_tbl_insert(priority, ptcb);

    return rc;
}



s16_t os_task_create(void (*task)(void *arg), 
                    void *arg,
                    os_stk_t *stack,
                    u8_t priority)
{
    os_stk_t *psp;
    os_tcb_t *ptcb;
    u16_t rc;

    os_cpu_psr_t cpu_psr = 0;

    if((priority > OS_CFG_LOWEST_PRIORITY) ||
       (priority == 0)){
        return OS_ERR_PRIORITY_INVALID;
    }

    if(k2os_task_priority_info_tbl[priority].ptcb != NULL){
        return OS_ERR_PRIORITY_EXIST;
    }

    os_enter_critical();
    if(k2os_isr_nesting > 0){   /* make sure don't create a task from within an isr */
        os_exit_critical();
        return OS_ERR_TASK_CREATE_WITHIN_ISR;
    }
    os_exit_critical();

    ptcb = os_tcb_malloc();
    if(ptcb == NULL){
        return OS_ERR_TASK_CREATE_MALLOC_TCB;
    }

    psp = port_task_stack_init(task, arg, stack);
    rc = os_task_tcb_init(ptcb, priority, psp);
    if(rc == OS_ERR_NONE){
        if(k2os_running){
            // os_sched();
        }
    }else{
        os_enter_critical();
        os_task_priority_info_tbl_delete(priority);   
        os_exit_critical();     
    }

    return rc;
}


static void os_task_ready_table_init(void)
{
    u8_t i;
    k2os_task_ready_group = 0;
    for(i = 0; i < OS_CFG_TASK_READY_TBL_SIZE; i++){
        k2os_task_ready_tbl[i] = 0;
    }

    k2os_curr_running_priority = 0;
    k2os_next_run_priority = 0;
}

/**
 *****************************************************************************************
 *                          system idle task init
 *****************************************************************************************
 */
static os_stk_t k2os_sys_idle_task_stack[OS_CFG_IDLE_TASK_STACK_SIZE];
volatile u32_t  k2os_sys_idle_task_cnt;

void os_sys_idle_task(void *arg)
{
    os_cpu_psr_t cpu_psr = 0;
    k2os_sys_idle_task_cnt = 0;

    while(1){
        os_enter_critical();
        k2os_sys_idle_task_cnt++;
        os_exit_critical();
    }
}

static void os_sys_idle_task_init(void)
{
    os_task_create(os_sys_idle_task, 
                   (void *)0,
                   &k2os_sys_idle_task_stack[OS_CFG_IDLE_TASK_STACK_SIZE - 1],
                   OS_CFG_LOWEST_PRIORITY);
}


static void os_common_init(void)
{
    k2os_ticks          = 0;
    k2os_isr_nesting    = 0;
    k2os_running        = 0;
}


void os_init(void)
{
    /* 0. common init for global variable */
    os_common_init();
    /* 1. os tcbs heaps init */
    os_tcb_heaps_init();
    /* 2. os task's priority info tbl init */
    os_task_priority_info_tbl_init();
    /* 3. os task ready table info */
    os_task_ready_table_init();
    /* 4. os system idle task init */
    os_sys_idle_task_init();
    /* 5. PendSV handle priority set */
    port_set_PendSV_Handler_lowest_priority();
}


void os_start(void)
{
    os_get_highest_ready_task_priority();
    os_update_next_run_task_ptcb();
    port_Systick_init();
    port_start_first_task();
}


void os_ticks_increment(void)
{
    os_cpu_psr_t cpu_psr = 0;
    os_tcb_t *ptcb = NULL;

    os_enter_critical();
    k2os_ticks++;
    os_exit_critical();

    tcb_slist_for_each_entry(ptcb){
        os_enter_critical();
        if(ptcb->taskDlyTicks > 0){
            ptcb->taskDlyTicks--;

            if(ptcb->taskDlyTicks == 0){
                if(ptcb->taskStatus == OS_TASK_STATUS_SLEEPING){
                    ptcb->taskStatus = OS_TASK_STATUS_READY;

                    k2os_task_ready_group                         |= ptcb->taskReadyGroup_mask;
                    k2os_task_ready_tbl[ptcb->taskReadyGroup_bit] |= ptcb->taskReadyTbl_mask;
                }
            }

        }
        os_exit_critical();
    }
}


static void os_sched_new(void)
{
    k2os_next_run_priority = os_get_highest_ready_task_priority();
    if(k2os_next_run_priority != k2os_curr_running_priority){
        os_update_next_run_task_ptcb();
        /* context switch is required, so Pend the PendSV interrupt*/
        port_trigger_PendSV_Handler();
    }
}


void os_isr_exit(void)
{
   os_cpu_psr_t cpu_psr = 0;

   if(k2os_running){
    os_enter_critical();
    if(k2os_isr_nesting > 0){
        k2os_isr_nesting--;
    }

    if(k2os_isr_nesting == 0){
        os_sched_new();
    }
    os_exit_critical();
   } 
}

void os_isr_nesting_inc(void)
{
    k2os_isr_nesting++;
}


void os_sched(void)
{
    os_cpu_psr_t cpu_psr = 0;

    os_enter_critical();
    if(k2os_isr_nesting == 0){
        os_sched_new();
    }
    os_exit_critical();
}


void os_time_dly(u32_t ticks)
{
    u8_t y;
    os_cpu_psr_t cpu_psr = 0;

    if(k2os_isr_nesting > 0){
        return;
    }

    if(ticks > 0){
        os_enter_critical();
        y = k2os_curr_running_task_ptcb->taskReadyGroup_bit;
        k2os_task_ready_tbl[y] &= ~k2os_curr_running_task_ptcb->taskReadyTbl_mask;
        if(k2os_task_ready_tbl[y] == 0){
            k2os_task_ready_group &= ~k2os_curr_running_task_ptcb->taskReadyGroup_mask;
        }
        k2os_curr_running_task_ptcb->taskDlyTicks = ticks;  
        k2os_curr_running_task_ptcb->taskStatus = OS_TASK_STATUS_SLEEPING;
        os_exit_critical();
        os_sched();                     /* find next task to run.*/
    }
}


s16_t os_task_dly(u32_t ms)
{
    u32_t ticks;

    if(k2os_isr_nesting > 0){
        return OS_ERR_TASK_DLY_ISR;
    }

    ticks = OS_CFG_TICKS_WITHIN_ONE_SECOND * (ms + 500 / OS_CFG_TICKS_WITHIN_ONE_SECOND) / 1000;

    os_time_dly(ticks);

    return OS_ERR_NONE;
}

void os_task_return(void)
{
    while (1){
        os_time_dly(OS_CFG_TICKS_WITHIN_ONE_SECOND);
    }
}
